#include <ProtocolHandler.h>

#include <SoftwareSerial.h>
#include <avr/pgmspace.h>
#include <math.h>
#include <sstream>
#include <stdlib.h>

//**********************
//   PINs set up
//********************** 

SoftwareSerial lcdSerial(8,7);
ProtocolHandler protocolHandler;

int temperatureSensor = A0;

const float arduino_power_supply = 5;
const float sensor_power_supply = 3.3;
const float zero_g_bias = sensor_power_supply / 2;
const float rad2deg = 57.2957795;

int accelerometerX = A3;
int accelerometerY = A4;
int accelerometerZ = A5;
int accelerometerST = A2;

int taster = 6;
int potentiometer = A1;

int ledRedTemperature = 4;
int ledYellowTemperature = 5;
int ledRedAccelerometer = 3;
int ledYellowAccelerometer = 2;

int avgTempCounter = 0;
float avgTemp;
float tempVals[10];

//**********************
//   Constants
//********************** 

enum {
  T_KELVIN=0,
  T_CELSIUS=1,
  T_FAHRENHEIT=2
};

/*float minT = -10.0;
float maxT = 50.0;
double accelXMin = -90.00;
double accelXMax = 90.00;
double accelYMin = -90.00;
double accelYMax = 90.00;
double accelZMin = -90.00;
double accelZMax = 90.00;
double dToverdtMax = 0.3;*/
float dToverdt = 0.0;
float dt = 0.0;
float dT = 0.0;
float time = 0.0;

String inputString;
boolean stringComplete = false;


//**********************
//   Readout values
//********************** 

float pmValue;
float x;
float y;
float z;
float pitch;
float roll;
float theta;
float tempInCelsius;
int resetBtn;
int accelStatus;

//**********************
//   SETUP
//********************** 

void setup(){
  setMinMaxValues();
  
  pinMode(temperatureSensor, INPUT);
  pinMode(accelerometerX, INPUT);
  pinMode(accelerometerY, INPUT);
  pinMode(accelerometerZ, INPUT);
  pinMode(accelerometerST, INPUT);
  
  pinMode(potentiometer, INPUT);
  
  pinMode(taster, INPUT);
  
  pinMode(ledRedTemperature, OUTPUT);
  pinMode(ledYellowTemperature, OUTPUT);
  pinMode(ledRedAccelerometer, OUTPUT);
  pinMode(ledYellowAccelerometer, OUTPUT);
  lcdSerial.begin(9600);
  backlightOn();
  selectLineOne();
  Serial.begin(9600);
  inputString.reserve(200);
}

//**********************
//   PROGRAM
//********************** 

void loop(){
  
  //  receive package in following format
  // better START | LEN | COMMAND | DATA | STOP
  //  where START = '0xEE' and STOP = '0xFF'
  if (stringComplete) {
    //Serial.println(inputString); 
    boolean ret = protocolHandler.handleRequest(inputString);
    // Serial.println(ret);
    // clear the string:
    inputString = "";
    stringComplete = false;
  }
  
  
  time = millis();
  
  //potentiometer readings
  pmValue = analogRead(potentiometer);
  accelStatus = analogRead(accelerometerST);

  x = voltage2g(analogRead(accelerometerX));
  y = voltage2g(analogRead(accelerometerY));
  z = voltage2g(analogRead(accelerometerZ));
  
  // the angle of the x-axis relative to ground
  pitch = atan(x/(sqrt(y*y+z*z)))*rad2deg;
  // the angle of the y-axis relative to the ground 
  roll = atan(y/(sqrt(x*x+z*z)))*rad2deg;
  // the angle of the z-axis relative to gravity
  theta = atan(sqrt(x*x+y*y)/z)*rad2deg;
  
  //temperature readings
  double temp = Temperature(0,T_CELSIUS,3740,298.15,22000,10000);
  //double temp = TemperatureWTable(0, 10000);
  dT = abs(tempInCelsius - temp);
  tempInCelsius = temp;
  
  //Serial.println(tempInCelsius);
  //reset button readings
  resetBtn = digitalRead(taster);
  if(resetBtn==0){
    setMinMaxValues();  
  }
  
  clearLCD();
  printValuesToLCD(pmValue);
  delay(200);
  
  dt = millis()-time;
  dToverdt = dT/dt;
  
  checkMinMaxValues();
  checkAlerts();
  checkMaxMinB4Reset();
  //avgTemperature(tempInCelsius);
  
}

//***************************
//   Alerts & Warnings Utils
//*************************** 

  void checkMinMaxValues(){ 
    if(tempInCelsius > protocolHandler.getMaxT(false)){
       digitalWrite(ledYellowTemperature, HIGH);
    }else if(dToverdt > protocolHandler.getDToverdfMax(false)){
      digitalWrite(ledYellowTemperature, HIGH);
    } else {
      digitalWrite(ledYellowTemperature, LOW);
    }
    
    if((abs(x)+abs(y)+abs(z))>protocolHandler.getAccelXMax(false)){
       digitalWrite(ledYellowAccelerometer, HIGH);
    } else {
       digitalWrite(ledYellowAccelerometer, LOW);
    }

  }
  void checkAlerts(){
    if (accelStatus){
      digitalWrite(ledRedAccelerometer, HIGH);
    }else{
      digitalWrite(ledRedAccelerometer, LOW);
    }
    
    if(analogRead(0)<=0){
      digitalWrite(ledRedTemperature, HIGH);
    } else {
      digitalWrite(ledRedTemperature, LOW);
    }
  }
  
  void checkMaxMinB4Reset(){
    if(tempInCelsius < protocolHandler.getminT_b4_reset()){
      protocolHandler.setminT_b4_reset(tempInCelsius);
    }
    if(tempInCelsius > protocolHandler.getmaxT_b4_reset()){
      protocolHandler.setmaxT_b4_reset(tempInCelsius);
    }
    if(x < protocolHandler.getaccelXMin_b4_reset()){
      protocolHandler.setaccelXMin_b4_reset(x);
    }  
    if(x > protocolHandler.getaccelXMax_b4_reset()){
      protocolHandler.setaccelXMax_b4_reset(x); 
    } 
    if(y < protocolHandler.getaccelYMin_b4_reset()){
      protocolHandler.setaccelYMin_b4_reset(y);
    }
    if(y > protocolHandler.getaccelYMax_b4_reset()){
      protocolHandler.setaccelYMax_b4_reset(y);
    } 
    if(z < protocolHandler.getaccelZMin_b4_reset()){
      protocolHandler.setaccelZMin_b4_reset(z);
    }
    if(z > protocolHandler.getaccelZMax_b4_reset()){
      protocolHandler.setaccelZMax_b4_reset(z);
    }
  }


//**********************
//   LCD Utils
//********************** 


void selectLineOne(){
  lcdSerial.write(0xFE);
  lcdSerial.write(128);
  delay(10); 
}

void backlightOn(){  
  lcdSerial.write(0x7C);   
  lcdSerial.write(157);   
  delay(10);
}

void backlightOff(){  
  lcdSerial.write(0x7C);
  lcdSerial.write(128);     
  delay(10);
}

void clearLCD(){
  lcdSerial.write(0xFE);   
  lcdSerial.write(0x01);
  delay(10);
}

void printToLCD(String message){
  lcdSerial.print(message);
}

void printToLCD(float message, int precision){
  lcdSerial.print(message,precision);
}

void printToLCD(double message, int precision){
  lcdSerial.print(message,precision);
}

void printValuesToLCD(float pmValue){
  if(pmValue>=0&&pmValue<341){
    printToLCD("X: ");printToLCD(x,4);
    printToLCD(" Y: ");printToLCD(y,4);
    printToLCD(" Z: ");printToLCD(z,4);
  }else if(pmValue>=341&&pmValue<682){
    printToLCD("T: ");printToLCD(tempInCelsius,2);printToLCD(" in C");
  } else if(pmValue>=682&&pmValue<=1023){ 
    printToLCD("p: ");printToLCD(pitch,2);
    printToLCD(" r:");printToLCD(roll,2);
    printToLCD(" t:");printToLCD(theta,2);
  }
}


//*****************
// Commons
//*****************

/*String ConvertFloatToString(float val, int val_size, int val_before_precision, int val_after_precision){
  char valStr[val_size];
  dtostrf(val,val_before_precision,val_after_precision,valStr);
  return String(valStr);
}

String ConvertDoubleToString(double val, int val_size, int val_before_precision, int val_after_precision){
  char valStr[val_size];
  dtostrf(val,val_before_precision,val_after_precision,valStr);
  return String(valStr);
}*/

//***********************
// Set MIN & MAX values
//***********************

void setMinMaxValues(){
  protocolHandler.setMinMaxValues(); 
}

//***************
// Temperature
//***************

//Ref: http://playground.arduino.cc/ComponentLib/Thermistor2
// Temperature function outputs float , the actual temperature
// Temperature function inputs
// 1. AnalogInputNumber - analog input to read from 
// 2. OuputUnit - output in celsius, kelvin or fahrenheit
// 3. Thermistor B parameter - found in datasheet 
// 4. Manufacturer T0 parameter - found in datasheet (kelvin)
// 5. Manufacturer R0 parameter - found in datasheet (ohms)
// 6. Your balance resistor resistance in ohms  

float Temperature(int AnalogInputNumber,int OutputUnit,float B,float T0,float R0,float R_Balance)
{
  return protocolHandler.getTemp(AnalogInputNumber, OutputUnit, B, T0, R0, R_Balance);
}

float TemperatureWTable(int AnalogInputNumber, float R_Balance)
{
  return protocolHandler.getTempWTable(AnalogInputNumber, R_Balance);
}

void avgTemperature(float temp){

  tempVals[avgTempCounter] = temp;
  avgTempCounter++;
  
  if(avgTempCounter==10){
    float sum;
    for(int i=0; i<10; i++){
      sum += tempVals[i];
    }
    avgTemp = sum/10;
    avgTempCounter = 0;
  }
}


//*****************
// Accelerometer
//*****************

double voltage2g(double accAnalogValue){
  double voltage = (accAnalogValue) * arduino_power_supply / 1024;
  return (voltage - zero_g_bias) * 1000 / 330;
}

//*****************
// Read serial
//*****************
void serialEvent(){
   while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read(); 
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
   }
}





